//#define ASSERT

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Threading;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;

namespace Oops.Xna.Framework.Physics
{
	/// <summary>
	/// Represents the behaviors and/or states of a <see cref="RigidBody />.
	/// </summary>
	[Flags()]
	public enum RigidBodyFlags
	{
		/// <summary>
		/// No special behavior or state.
		/// </summary>
		None = 0,

		/// <summary>
		/// The <see cref="RigidBody"/> is disabled.
		/// </summary>
		Disabled = 1,

		/// <summary>
		/// The <see cref="RigidBody"/> can be auto disabled.
		/// </summary>
		AutoDisable = 2,

		/// <summary>
		/// The <see cref="RigidBody"/> is immovable.
		/// </summary>
		Static = 4,

		/// <summary>
		/// The <see cref="RigidBody" /> is unaffected by collisions.
		/// </summary>
		NoResponse = 8
	}

	/// <summary>
	/// Represents a single solid body of finite size.
	/// </summary>
	public class RigidBody
	{
		#region ConstraintCollection Declaration

		// Is this the correct association???  Will CollisionGroups ever need an association???
		/// <summary>
		/// A collection of <see cref="Constaint"/> instances.
		/// </summary>
		public sealed class ConstraintCollection : ReadOnlyCollection<Constraint>
		{
			/// <summary>
			/// Initializes a new instance of the <see cref="ConstraintCollection"/> class.
			/// </summary>
			internal ConstraintCollection() : base(new List<Constraint>()) { }

			/// <summary>
			/// Exposes the <see cref="System.Collections.Generic.IList[T]" /> that the <see cref="ConstraintCollection"/> wraps.
			/// </summary>
			internal protected new IList<Constraint> Items
			{
				get { return base.Items; }
			}

			/// <summary>
			/// Marks all constraints as unsolved.
			/// </summary>
			internal void Reset()
			{
				for (int i = 0; i < this.Count; i++)
				{
					this.Items[i].satisfied = false;
				}
			}
		}

		#endregion

		private static int instanceCount = 0;
		private int id;

		/// <summary>
		/// Gets or sets the behavior flags of the <see cref="RigidBody"/>.
		/// </summary>
		public RigidBodyFlags Flags = RigidBodyFlags.AutoDisable;

		private CollisionGroup collisionGroup;
		private ConstraintCollection constraints = new ConstraintCollection();

		private float mass = 1.0f;
		internal float inverseMass = 1.0f;

		private Matrix inertiaTensor;
		private Matrix inverseInertiaTensor;
		internal Matrix inverseInertiaTensorWorld;

		/// <summary>
		/// Gets the location of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		/// <remarks>
		/// Do not set this field directly.  Instead, use the <see cref="RigidBody.UpdateTransform" /> method.
		/// </remarks>
		public Vector3 Position;

		/// <summary>
		/// Gets the orientation of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		/// <remarks>
		/// Do not set this field directly.  Instead, use the <see cref="RigidBody.UpdateTransform" /> method.
		/// </remarks>
#if QUATERNION_ORIENTATION 
		public Quaternion Orientation = Quaternion.Identity;
#else
		public Matrix Orientation = Matrix.Identity;
#endif
		/// <summary>
		/// Gets or sets the linear velocity of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		/// <remarks>
		/// The velocity of the <see cref="RigidBody"/> is normally adjusted through integration and application of forces/impulses.
		/// A better but indirect approach to changing the velocity is applying a force or impulse using the exposed methods
		/// of the <see cref="RigidBody" /> instance.
		/// </remarks>
		public Vector3 Velocity;

		/// <summary>
		/// Gets or sets the angular velocity of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		/// <remarks>
		/// The rotation of the rigid body is normally adjusted through integration and application of forces/impulses.
		/// A better but indirect approach to changing the rotation is applying torque or angular impulse using the exposed methods
		/// of the <see cref="RigidBody" /> instance.
		/// </remarks>
		public Vector3 Rotation;

		/// <summary>
		/// Gets or sets the instantaneous force of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		public Vector3 Force;

		/// <summary>
		/// Gets or sets the instantaneous torque, i.e. angular force, of the <see cref="RigidBody"/> in world coordinates.
		/// </summary>
		public Vector3 Torque;

		/// <summary>
		/// Gets or sets the linear force applied at every time frame to the <see cref="RigidBody"/>.
		/// </summary>
		/// <remarks>
		/// Normally, this vector represents the force due to gravity in units of measurement per second.
		/// </remarks>
		public Vector3 Acceleration = Vector3.Down * 13.0f;

		/// <summary>
		/// Gets or sets the linear damping factor to apply to the <see cref="RigidBody"/> during integration.
		/// </summary>
		public float LinearDamping;

		/// <summary>
		/// Gets or sets the angular damping factor to apply to the <see cref="RigidBody"/> during integration.
		/// </summary>
		public float AngularDamping;

		private float inactiveTime;

		private float linearMotionThreshold;
		private float linearMotionThresholdSquared;
		private float angularMotionThreshold;
		private float angularMotionThresholdSquared;

		// TODO : Not used yet.
		private float velocityLimit;
		private float velocityLimitSquared;
		private float rotationLimit;
		private float rotationLimitSquared;

		/// <summary>
		/// Initializes a new instance of the <see cref="RigidBody" /> class.
		/// </summary>
		public RigidBody()
		{
			// Assign the rigid body a unique id.
			this.id = Interlocked.Increment(ref RigidBody.instanceCount);

			// Create the associated collision group collection.
			this.collisionGroup = new CollisionGroup(this);

			// Default some properties.
			this.DeactivationTime = 1.25f;
			this.LinearMotionThreshold = 0.6f;
			this.AngularMotionThreshold = (MathHelper.Pi / 8.0f);
		}

		/// <summary>
		/// Gets a unique id for the <see cref="RigidBody" />.
		/// </summary>
		public int Id
		{
			get { return this.id; }
		}

		/// <summary>
		/// Gets or sets the object that contains data about the <see cref="RigidBody"/>.
		/// </summary>
		[ContentSerializerIgnore()]
		public object Tag { get; set; }

		/// <summary>
		/// Gets or sets the total mass of the <see cref="RigidBody" />.
		/// </summary>
		/// <remarks>
		/// The mass of a <see cref="RigidBody" /> is calculated from the defined density, 
		/// associated <see cref="CollisionGroup" /> and it's collision shapes.
		/// </remarks>
		public float Mass
		{
			get { return this.mass; }
			set
			{
				// Hold onto value.
				this.mass = value;

				// Hold onto the mass's inverse.  Most calculation involving mass use the inverse.
				this.inverseMass = 1.0f / this.mass;
			}
		}

		/// <summary>
		/// Gets or sets the combined moment of inertia of the <see cref="RigidBody" />.
		/// </summary>
		/// <remarks>
		/// The inertia tensor of a <see cref="RigidBody" /> is calculated from the mass, 
		/// associated <see cref="CollisionGroup" /> and it's collision shapes.
		/// </remarks>
		public Matrix InertiaTensor
		{
			get { return this.inertiaTensor; }
			set
			{
				// Hold onto value.
				this.inertiaTensor = value;

				// Hold onto the inertia tensor's inverse.  Most calculation involving moment of inertia use the inverse.
				if (this.inertiaTensor == MatrixExtension.Zero)
				{
					this.inverseInertiaTensor = MatrixExtension.Zero;
				}
				else if (this.inertiaTensor.M11 == 0.0f || this.inertiaTensor.M22 == 0.0f || this.inertiaTensor.M33 == 0.0f)
				{
					this.inverseInertiaTensor.M11 = this.inertiaTensor.M11 == 0.0f ? this.inertiaTensor.M11 : 1.0f / this.inertiaTensor.M11;
					this.inverseInertiaTensor.M22 = this.inertiaTensor.M22 == 0.0f ? this.inertiaTensor.M22 : 1.0f / this.inertiaTensor.M22;
					this.inverseInertiaTensor.M33 = this.inertiaTensor.M33 == 0.0f ? this.inertiaTensor.M33 : 1.0f / this.inertiaTensor.M33;
				}
				else
				{
					MatrixExtension.Invert33(ref this.inertiaTensor, out this.inverseInertiaTensor);
				}
			}
		}

		/// <summary>
		/// Gets the associated <see cref="CollisionGroup" /> of the <see cref="RigidBody" />.
		/// </summary>
		public CollisionGroup CollisionGroup
		{
			get { return this.collisionGroup; }
		}

		/// <summary>
		/// Gets the associated <see cref="Constraint" /> instances of the <see cref="RigidBody" />.
		/// </summary>
		/// <remarks>
		/// Constraints are associated with an active <see cref="RigidBody"/> when added to 
		/// the <see cref="IPhysicsService.Constraints"/> collection.
		/// </remarks>
		[ContentSerializerIgnore()]
		public ConstraintCollection Constraints
		{
			get { return this.constraints; }
		}

		/// <summary>
		/// Gets or sets the time the <see cref="RigidBody" /> must remain under the specified movement thresholds in order
		/// to be considered for deactivation.
		/// </summary>
		/// <remarks>
		/// The value of the this property is considered only when the <see cref="RigidBody.Flags"/> field 
		/// contains <see cref="RigidBodyFlags.AutoDisable"/>.  The value represents seconds.
		/// </remarks>
		public float DeactivationTime { get; set; }

		/// <summary>
		/// Gets or sets the threshold for linear motion of the <see cref="RigidBody" />.
		/// </summary>
		/// <remarks>
		/// When motion of the <see cref="RigidBody"/> remains under this and the <see cref="RigidBody.AngularMotionThreshold"/> thresholds
		/// for the specified <see cref="RigidBody.DeactivationTime"/>, the <see cref="RigidBody"/>
		/// will be deactivated.
		/// </remarks>
		public float LinearMotionThreshold
		{
			get { return this.linearMotionThreshold; }
			set
			{
				// Hold onto the value.
				this.linearMotionThreshold = value;

				// Hold onto the square of the motion threshold.
				this.linearMotionThresholdSquared = value * value;
			}
		}

		/// <summary>
		/// Gets or sets the threshold for angular motion of the <see cref="RigidBody" />.
		/// </summary>
		/// <remarks>
		/// When motion of the <see cref="RigidBody"/> remains under this and the <see cref="RigidBody.LinearMotionThreshold"/> thresholds
		/// for the specified <see cref="RigidBody.DeactivationTime"/>, the <see cref="RigidBody"/>
		/// will be deactivated.
		/// </remarks>
		public float AngularMotionThreshold
		{
			get { return this.angularMotionThreshold; }
			set
			{
				// Hold onto the value.
				this.angularMotionThreshold = value;

				// Hold onto the square of the motion threshold.
				this.angularMotionThresholdSquared = value * value;
			}
		}

		/// <summary>
		/// Gets or sets the limit for velocity of the <see cref="RigidBody" />.
		/// </summary>
		public float VelocityLimit
		{
			get { return this.velocityLimit; }
			set
			{
				// Hold onto the value.
				this.velocityLimit = value;

				// Hold onto the square of the limit.
				this.velocityLimitSquared = value * value;
			}
		}

		/// <summary>
		/// Gets or sets the limit for rotation of the <see cref="RigidBody" />.
		/// </summary>
		public float RotationLimit
		{
			get { return this.rotationLimit; }
			set
			{
				// Hold onto the value.
				this.rotationLimit = value;

				// Hold onto the square of the limit.
				this.rotationLimitSquared = value * value;
			}
		}

		/// <summary>
		/// Integrates the velocities from the forces applied to the <see cref="RigidBody" />.
		/// </summary>
		/// <param name="elapsed">The simulation step's total elapsed time, in seconds.</param>
		internal void IntegrateVelocity(float elapsed)
		{
			this.Assert();

			// Make sure the rigid body is enabled and not static.
			if ((this.Flags & (RigidBodyFlags.Disabled | RigidBodyFlags.Static)) != RigidBodyFlags.None)
			{
				// Exit.
				return;
			}
#if INLINE
			// Calculate the current linear acceleration from the applied forces and constant acceleration (gravity)
			// during this iteration.  Hold onto this value for collision resolution calculations.
			float ax = (this.Acceleration.X + this.Force.X * this.inverseMass) * elapsed;
			float ay = (this.Acceleration.Y + this.Force.Y * this.inverseMass) * elapsed;
			float az = (this.Acceleration.Z + this.Force.Z * this.inverseMass) * elapsed;

			// Update the linear velocity from linear acceleration.
			this.Velocity.X = this.Velocity.X + ax;
			this.Velocity.Y = this.Velocity.Y + ay;
			this.Velocity.Z = this.Velocity.Z + az;

			// Calculate the current angular acceleration from the applied torque during this iteration.
			float x = this.Torque.X * elapsed;
			float y = this.Torque.Y * elapsed;
			float z = this.Torque.Z * elapsed;

			// Update the rotation from angular acceleration.
			this.Rotation.X = this.Rotation.X + (x * this.inverseInertiaTensorWorld.M11 + y * this.inverseInertiaTensorWorld.M21 + z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (x * this.inverseInertiaTensorWorld.M12 + y * this.inverseInertiaTensorWorld.M22 + z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (x * this.inverseInertiaTensorWorld.M13 + y * this.inverseInertiaTensorWorld.M23 + z * this.inverseInertiaTensorWorld.M33);

			this.Assert();

			// Adjust the linear and angular velocities by the linear and angular dampings, respectively.
			float temp = MathHelper.Clamp(1.0f - elapsed * this.LinearDamping, 0.0f, 1.0f);
			this.Velocity.X = this.Velocity.X * temp;
			this.Velocity.Y = this.Velocity.Y * temp;
			this.Velocity.Z = this.Velocity.Z * temp;

			temp = MathHelper.Clamp(1.0f - elapsed * this.AngularDamping, 0.0f, 1.0f);
			this.Rotation.X = this.Rotation.X * temp;
			this.Rotation.Y = this.Rotation.Y * temp;
			this.Rotation.Z = this.Rotation.Z * temp;

			this.Assert();
#elif REFERENCE
			// Calculate the current linear acceleration from the applied forces and constant acceleration (gravity)
			// during this iteration.  Hold onto this value for collision resolution calculations.
			Vector3 currentAcceleration;
			Vector3.Multiply(ref this.Force, this.inverseMass, out currentAcceleration);
			Vector3.Add(ref currentAcceleration, ref this.Acceleration, out currentAcceleration);
			Vector3.Multiply(ref currentAcceleration, elapsed, out currentAcceleration);

			// Calculate the current angular acceleration from the applied torque during this iteration.
			Vector3 angularAcceleration;
			Vector3.Multiply(ref this.Torque, elapsed, out angularAcceleration);
			Vector3.TransformNormal(ref angularAcceleration, ref this.inverseInertiaTensorWorld, out angularAcceleration);

			// Update the linear and angular velocities from linear and angular accelerations, respectively.
			Vector3.Add(ref this.Velocity, ref currentAcceleration, out this.Velocity);
			Vector3.Add(ref this.Rotation, ref angularAcceleration, out this.Rotation);

			this.Assert();

			// Adjust the linear and angular velocities by the linear and angular dampings, respectively.
			Vector3.Multiply(ref this.Velocity, MathHelper.Clamp(1.0f - elapsed * this.LinearDamping, 0.0f, 1.0f), out this.Velocity);
			Vector3.Multiply(ref this.Rotation, MathHelper.Clamp(1.0f - elapsed * this.AngularDamping, 0.0f, 1.0f), out this.Rotation);

			this.Assert();
#else
			// Calculate the current linear acceleration from the applied forces and constant acceleration (gravity)
			// during this iteration.  Hold onto this value for collision resolution calculations.
			Vector3 currentAcceleration = (this.Acceleration + this.Force * this.inverseMass) * elapsed;

			// Calculate the current angular acceleration from the applied torque during this iteration.
			Vector3 angularAcceleration = Vector3.TransformNormal(this.Torque * elapsed, this.inverseInertiaTensorWorld);

			// Update the linear and angular velocities from linear and angular accelerations, respectively.
			this.Velocity += currentAcceleration;
			this.Rotation += angularAcceleration;

			this.Assert();

			// Adjust the linear and angular velocities by the linear and angular dampings, respectively.
			this.Velocity *= MathHelper.Clamp(1.0f - this.LinearDamping * elapsed, 0.0f, 1.0f);
			this.Rotation *= MathHelper.Clamp(1.0f - this.AngularDamping * elapsed, 0.0f, 1.0f);

			this.Assert();
#endif
		}

		/// <summary>
		/// Integrates the transformation from the velocities applied to the <see cref="RigidBody" />.
		/// </summary>
		/// <param name="elapsed">The simulation step's total elapsed time, in seconds.</param>
		internal void IntegratePosition(float elapsed)
		{
			// Make sure the rigid body is enabled and not static.
			if ((this.Flags & (RigidBodyFlags.Disabled | RigidBodyFlags.Static)) != RigidBodyFlags.None)
			{
				// Exit.
				return;
			}
#if QUATERNION_ORIENTATION
			// Update the position by the velocity.
			this.Position += this.Velocity * elapsed;
#if false
			// Update the orientation by the rotation.
			float length = this.Rotation.Length();
			if (length > MathExtension.Epsilon)
			{
				this.Orientation = Quaternion.CreateFromAxisAngle(this.Rotation / length, length * elapsed) * this.Orientation;
			}
#else
			// Update the orientation by the rotation.
			Quaternion rotationQ = new Quaternion(this.Rotation * elapsed, 0.0f);
			Quaternion.Multiply(ref rotationQ, ref this.Orientation, out rotationQ);
			this.Orientation += rotationQ * 0.5f;
			this.Orientation.Normalize();
#endif	
			// Calculate the inertia tensor in world coordinates.
			this.inverseInertiaTensorWorld = MatrixExtension.ConvertBasis33(this.inverseInertiaTensor, Matrix.CreateFromQuaternion(this.Orientation));
#else
#if INLINE
			// Update the position by the velocity.
			this.Position.X = this.Position.X + this.Velocity.X * elapsed;
			this.Position.Y = this.Position.Y + this.Velocity.Y * elapsed;
			this.Position.Z = this.Position.Z + this.Velocity.Z * elapsed;

			// Update the orientation by the rotation.
			float length = (float)Math.Sqrt(this.Rotation.X * this.Rotation.X + this.Rotation.Y * this.Rotation.Y + this.Rotation.Z * this.Rotation.Z);
			if (length > MathExtension.Epsilon)
			{
#if XBOX
				Vector3 axis = Vector3.Zero;
#else
				Vector3 axis;
#endif
				float temp = 1.0f / length;
				axis.X = this.Rotation.X * temp;
				axis.Y = this.Rotation.Y * temp;
				axis.Z = this.Rotation.Z * temp;
				
				Matrix matrix;
				Matrix.CreateFromAxisAngle(ref axis, length * elapsed, out matrix);

				float n11 = this.Orientation.M11 * matrix.M11 + this.Orientation.M12 * matrix.M21 + this.Orientation.M13 * matrix.M31;
				float n12 = this.Orientation.M11 * matrix.M12 + this.Orientation.M12 * matrix.M22 + this.Orientation.M13 * matrix.M32;
				float n13 = this.Orientation.M11 * matrix.M13 + this.Orientation.M12 * matrix.M23 + this.Orientation.M13 * matrix.M33;
				
				float n21 = this.Orientation.M21 * matrix.M11 + this.Orientation.M22 * matrix.M21 + this.Orientation.M23 * matrix.M31;
				float n22 = this.Orientation.M21 * matrix.M12 + this.Orientation.M22 * matrix.M22 + this.Orientation.M23 * matrix.M32;
				float n23 = this.Orientation.M21 * matrix.M13 + this.Orientation.M22 * matrix.M23 + this.Orientation.M23 * matrix.M33;
				
				float n31 = this.Orientation.M31 * matrix.M11 + this.Orientation.M32 * matrix.M21 + this.Orientation.M33 * matrix.M31;
				float n32 = this.Orientation.M31 * matrix.M12 + this.Orientation.M32 * matrix.M22 + this.Orientation.M33 * matrix.M32;
				float n33 = this.Orientation.M31 * matrix.M13 + this.Orientation.M32 * matrix.M23 + this.Orientation.M33 * matrix.M33;
								
				this.Orientation.M11 = n11;
				this.Orientation.M12 = n12;
				this.Orientation.M13 = n13;
				this.Orientation.M14 = 0.0f;
									 
				this.Orientation.M21 = n21;
				this.Orientation.M22 = n22;
				this.Orientation.M23 = n23;
				this.Orientation.M24 = 0.0f;
									 
				this.Orientation.M31 = n31;
				this.Orientation.M32 = n32;
				this.Orientation.M33 = n33;
				this.Orientation.M34 = 0.0f;

				this.Orientation.M41 = 0.0f;
				this.Orientation.M42 = 0.0f;
				this.Orientation.M43 = 0.0f;
				this.Orientation.M44 = 1.0f;

				MatrixExtension.Orthonormalize(ref this.Orientation);
			}

			// Calculate the inertia tensor in world coordinates.
			float m11 = this.Orientation.M11 * this.inverseInertiaTensor.M11 + this.Orientation.M21 * this.inverseInertiaTensor.M12 + this.Orientation.M31 * this.inverseInertiaTensor.M13;
			float m12 = this.Orientation.M11 * this.inverseInertiaTensor.M21 + this.Orientation.M21 * this.inverseInertiaTensor.M22 + this.Orientation.M31 * this.inverseInertiaTensor.M23;
			float m13 = this.Orientation.M11 * this.inverseInertiaTensor.M31 + this.Orientation.M21 * this.inverseInertiaTensor.M32 + this.Orientation.M31 * this.inverseInertiaTensor.M33;

			float m21 = this.Orientation.M12 * this.inverseInertiaTensor.M11 + this.Orientation.M22 * this.inverseInertiaTensor.M12 + this.Orientation.M32 * this.inverseInertiaTensor.M13;
			float m22 = this.Orientation.M12 * this.inverseInertiaTensor.M21 + this.Orientation.M22 * this.inverseInertiaTensor.M22 + this.Orientation.M32 * this.inverseInertiaTensor.M23;
			float m23 = this.Orientation.M12 * this.inverseInertiaTensor.M31 + this.Orientation.M22 * this.inverseInertiaTensor.M32 + this.Orientation.M32 * this.inverseInertiaTensor.M33;

			float m31 = this.Orientation.M13 * this.inverseInertiaTensor.M11 + this.Orientation.M23 * this.inverseInertiaTensor.M12 + this.Orientation.M33 * this.inverseInertiaTensor.M13;
			float m32 = this.Orientation.M13 * this.inverseInertiaTensor.M21 + this.Orientation.M23 * this.inverseInertiaTensor.M22 + this.Orientation.M33 * this.inverseInertiaTensor.M23;
			float m33 = this.Orientation.M13 * this.inverseInertiaTensor.M31 + this.Orientation.M23 * this.inverseInertiaTensor.M32 + this.Orientation.M33 * this.inverseInertiaTensor.M33;

			this.inverseInertiaTensorWorld.M11 = m11 * this.Orientation.M11 + m12 * this.Orientation.M21 + m13 * this.Orientation.M31;
			this.inverseInertiaTensorWorld.M21 = m11 * this.Orientation.M12 + m12 * this.Orientation.M22 + m13 * this.Orientation.M32;
			this.inverseInertiaTensorWorld.M31 = m11 * this.Orientation.M13 + m12 * this.Orientation.M23 + m13 * this.Orientation.M33;
			this.inverseInertiaTensorWorld.M41 = 0.0f;

			this.inverseInertiaTensorWorld.M12 = m21 * this.Orientation.M11 + m22 * this.Orientation.M21 + m23 * this.Orientation.M31;
			this.inverseInertiaTensorWorld.M22 = m21 * this.Orientation.M12 + m22 * this.Orientation.M22 + m23 * this.Orientation.M32;
			this.inverseInertiaTensorWorld.M32 = m21 * this.Orientation.M13 + m22 * this.Orientation.M23 + m23 * this.Orientation.M33;
			this.inverseInertiaTensorWorld.M42 = 0.0f;

			this.inverseInertiaTensorWorld.M13 = m31 * this.Orientation.M11 + m32 * this.Orientation.M21 + m33 * this.Orientation.M31;
			this.inverseInertiaTensorWorld.M23 = m31 * this.Orientation.M12 + m32 * this.Orientation.M22 + m33 * this.Orientation.M32;
			this.inverseInertiaTensorWorld.M33 = m31 * this.Orientation.M13 + m32 * this.Orientation.M23 + m33 * this.Orientation.M33;
			this.inverseInertiaTensorWorld.M43 = 0.0f;

			this.inverseInertiaTensorWorld.M14 = 0.0f;
			this.inverseInertiaTensorWorld.M24 = 0.0f;
			this.inverseInertiaTensorWorld.M34 = 0.0f;
			this.inverseInertiaTensorWorld.M44 = 1.0f;
#elif REFERENCE
			// Update the position by the velocity.
			Vector3 temp;
			Vector3.Multiply(ref this.Velocity, elapsed, out temp);
			Vector3.Add(ref this.Position, ref temp, out this.Position);

			// Update the orientation by the rotation.
			float length = this.Rotation.Length();
			if (length > MathExtension.Epsilon)
			{
				Vector3 axis;
				Vector3.Divide(ref this.Rotation, length, out axis);
				
				Matrix matrix;
				Matrix.CreateFromAxisAngle(ref axis, length * elapsed, out matrix);
				Matrix.Multiply(ref this.Orientation, ref matrix, out this.Orientation);
				MatrixExtension.Orthonormalize(ref this.Orientation);
			}

			// Calculate the inertia tensor in world coordinates.
			MatrixExtension.ConvertBasis33(ref this.inverseInertiaTensor, ref this.Orientation, out this.inverseInertiaTensorWorld);
#else
			// Update the position by the velocity.
			this.Position += this.Velocity * elapsed;

			// Update the orientation by the rotation.
			float length = this.Rotation.Length();
			if (length > MathExtension.Epsilon)
			{
				this.Orientation *= Matrix.CreateFromAxisAngle(this.Rotation / length, length * elapsed);
				MatrixExtension.Orthonormalize(ref this.Orientation);
			}

			// Calculate the inertia tensor in world coordinates.
			this.inverseInertiaTensorWorld = MatrixExtension.ConvertBasis33(this.inverseInertiaTensor, this.Orientation);
#endif
#endif
			this.Assert();

			// Calculate all associated collision shape's internal data.
			this.collisionGroup.UpdateTransform(ref this.Position, ref this.Orientation);
		}

		/// <summary>
		/// Updates the <see cref="RigidBody" /> to the specified transformation.
		/// </summary>
		/// <remarks>
		/// Use the <see cref="RigidBody.UpdateTransform"/> method instead of setting the <see cref="RigidBody.Position" /> field or
		/// <see cref="RigidBody.Orientation"/> explicitly.
		/// </remarks>
		public void UpdateTransform()
		{
			// Call overloaded method.
			this.UpdateTransform(ref this.Position, ref this.Orientation);
		}

		/// <summary>
		/// Updates the <see cref="RigidBody" /> to the specified transformation.
		/// </summary>
		/// <param name="position">The location in world coordinates to where the <see cref="RigidBody"/> should be placed.</param>
		/// <remarks>
		/// Use the <see cref="RigidBody.UpdateTransform"/> method instead of setting the <see cref="RigidBody.Position" /> fields explicitly.
		/// </remarks>
		public void UpdateTransform(ref Vector3 position)
		{
			// Call overloaded method.
			this.UpdateTransform(ref position, ref this.Orientation);
		}

		/// <summary>
		/// Updates the <see cref="RigidBody" /> to the specified transformation.
		/// </summary>
		/// <param name="orientation">The orientation in world coordinates to place the <see cref="RigidBody"/>.</param>
		/// <remarks>
		/// Use the <see cref="RigidBody.UpdateTransform"/> method instead of setting the <see cref="RigidBody.Orientation" /> fields explicitly.
		/// </remarks>
#if QUATERNION_ORIENTATION
		public void UpdateTransform(ref Quaternion orientation)
#else
		public void UpdateTransform(ref Matrix orientation)
#endif
		{
			// Call overloaded method.
			this.UpdateTransform(ref this.Position, ref orientation);
		}

		/// <summary>
		/// Updates the <see cref="RigidBody" /> to the specified transformation.
		/// </summary>
		/// <param name="position">The location in world coordinates to where the <see cref="RigidBody"/> should be placed.</param>
		/// <param name="orientation">The orientation in world coordinates to place the <see cref="RigidBody"/>.</param>
		/// <remarks>
		/// Use the <see cref="RigidBody.UpdateTransform"/> method instead of setting the <see cref="RigidBody.Position" /> and 
		/// <see cref="RigidBody.Orientation"/> fields explicitly.
		/// </remarks>
#if QUATERNION_ORIENTATION
		public void UpdateTransform(ref Vector3 position, ref Quaternion orientation)
#else
		public void UpdateTransform(ref Vector3 position, ref Matrix orientation)
#endif
		{
			// Make sure the rigid body is active.
			this.Activate();

			// Modify the current transformation of the rigid body.
			this.Position = position;
			this.Orientation = orientation;

			this.Assert();

			// Move all associated collision shapes as well.
			this.collisionGroup.UpdateTransform(ref this.Position, ref this.Orientation);
		}

		/// <summary>
		/// Activates the <see cref="RigidBody" /> from a disabled state.
		/// </summary>
		public void Activate()
		{
			if ((this.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
			{
				this.Flags &= ~RigidBodyFlags.Disabled;
				this.inactiveTime = 0.0f;
			}
		}

		/// <summary>
		/// Deactivates the <see cref="RigidBody" /> from an enabled state.
		/// </summary>
		public void Deactivate()
		{
			if ((this.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.None)
			{
				this.Flags |= RigidBodyFlags.Disabled;
				this.inactiveTime = this.DeactivationTime;

				// Clear the velocities as well.
				this.Velocity = Vector3.Zero;
				this.Rotation = Vector3.Zero;
			}
		}

		/// <summary>
		/// Updates the inactivity time based on movement and deactivates the <see cref="RigidBody"/> if necessary.
		/// </summary>
		/// <param name="elapsed">The simulation step's total elapsed time.</param>
		internal void AttemptDeactivation(float elapsed)
		{
			if ((this.Flags & RigidBodyFlags.AutoDisable) == RigidBodyFlags.AutoDisable)
			{
				// Determine if motion is under tolerances.
#if INLINE
				if (this.Velocity.X * this.Velocity.X + this.Velocity.Y * this.Velocity.Y + this.Velocity.Z * this.Velocity.Z > this.linearMotionThresholdSquared ||
					this.Rotation.X * this.Rotation.X + this.Rotation.Y * this.Rotation.Y + this.Rotation.Z * this.Rotation.Z > this.angularMotionThresholdSquared)
#else
				if (this.Velocity.LengthSquared() > this.linearMotionThresholdSquared ||
					this.Rotation.LengthSquared() > this.angularMotionThresholdSquared)
#endif
				{
					this.inactiveTime = 0.0f;
				}
				else
				{
					this.inactiveTime += elapsed;
				}

				// Check if the rigid body should be deactivated.
				if (this.inactiveTime >= this.DeactivationTime)
				{
					this.Deactivate();
				}
			}
		}

		/// <summary>
		/// Clears the applied force and torque of the <see cref="RigidBody" />.
		/// </summary>
		public void ClearForces()
		{
			// Clear the force and torque.  Normally happens after each integration but manual reasons exist.
			this.Force = Vector3.Zero;
			this.Torque = Vector3.Zero;
		}

		/// <summary>
		/// Applies a force in local coordinates to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="force">The force, in local-coordinates to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyLocalForce(Vector3 force)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified force to the total.
#if QUATERNION_ORIENTATION
			this.Force += Vector3.Transform(force, this.Orientation);
#else
#if INLINE
			float x = force.X * this.Orientation.M11 + force.Y * this.Orientation.M21 + force.Z * this.Orientation.M31;
			float y = force.X * this.Orientation.M12 + force.Y * this.Orientation.M22 + force.Z * this.Orientation.M32;
			float z = force.X * this.Orientation.M13 + force.Y * this.Orientation.M23 + force.Z * this.Orientation.M33;
    
			this.Force.X = this.Force.X + x;
			this.Force.Y = this.Force.Y + y;
			this.Force.Z = this.Force.Z + z;
#elif REFERENCE
			Vector3.TransformNormal(ref force, ref this.Orientation, out force);
			Vector3.Add(ref this.Force, ref force, out this.Force);
#else
			this.Force += Vector3.TransformNormal(force, this.Orientation);
#endif
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies a force in local coordinates to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="force">The force, in local-coordinates to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyLocalForce(ref Vector3 force)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified force to the total.
#if QUATERNION_ORIENTATION
			this.Force += Vector3.Transform(force, this.Orientation);
#else
#if INLINE
			float x = force.X * this.Orientation.M11 + force.Y * this.Orientation.M21 + force.Z * this.Orientation.M31;
			float y = force.X * this.Orientation.M12 + force.Y * this.Orientation.M22 + force.Z * this.Orientation.M32;
			float z = force.X * this.Orientation.M13 + force.Y * this.Orientation.M23 + force.Z * this.Orientation.M33;

			this.Force.X = this.Force.X + x;
			this.Force.Y = this.Force.Y + y;
			this.Force.Z = this.Force.Z + z;
#elif REFERENCE
			Vector3 temp;
			Vector3.TransformNormal(ref force, ref this.Orientation, out temp);
			Vector3.Add(ref this.Force, ref temp, out this.Force);
#else
			this.Force += Vector3.TransformNormal(force, this.Orientation);
#endif
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies a force to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="force">The force, in world-coordinates to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyWorldForce(Vector3 force)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified force to the total.
#if INLINE
			this.Force.X = this.Force.X + force.X;
			this.Force.Y = this.Force.Y + force.Y;
			this.Force.Z = this.Force.Z + force.Z;
#elif REFERENCE
			Vector3.Add(ref this.Force, ref force, out this.Force);
#else
			this.Force += force;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies a force to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="force">The force, in world-coordinates to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyWorldForce(ref Vector3 force)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified force to the total.
#if INLINE
			this.Force.X = this.Force.X + force.X;
			this.Force.Y = this.Force.Y + force.Y;
			this.Force.Z = this.Force.Z + force.Z;
#elif REFERENCE
			Vector3.Add(ref this.Force, ref force, out this.Force);
#else
			this.Force += force;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies a force to the <see cref="RigidBody" /> at the specified location in world-coordinates.
		/// </summary>
		/// <param name="force">The force, in world-coordinates to apply to the <see cref="RigidBody"/>.</param>
		/// <param name="position">The location, in world-coordinates, to apply the force.</param>
		public void ApplyWorldForce(Vector3 force, Vector3 position)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Apply the specified force at the specified location to the total.
#if INLINE
			this.Force.X = this.Force.X + force.X;
			this.Force.Y = this.Force.Y + force.Y;
			this.Force.Z = this.Force.Z + force.Z;

			position.X = position.X - this.Position.X;
			position.Y = position.Y - this.Position.Y;
			position.Z = position.Z - this.Position.Z;

			this.Torque.X = this.Torque.X + (position.Y * force.Z - position.Z * force.Y);
			this.Torque.Y = this.Torque.Y + (position.Z * force.X - position.X * force.Z);
			this.Torque.Z = this.Torque.Z + (position.X * force.Y - position.Y * force.X);
#elif REFERENCE
			Vector3.Add(ref this.Force, ref force, out this.Force);

			Vector3.Subtract(ref position, ref this.Position, out position);
			Vector3.Cross(ref position, ref force, out position);
			Vector3.Add(ref this.Torque, ref position, out this.Torque);
#else
			this.Force += force;
			this.Torque += Vector3.Cross(position - this.Position, force);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies a force to the <see cref="RigidBody" /> at the specified location in world-coordinates.
		/// </summary>
		/// <param name="force">The force, in world-coordinates to apply to the <see cref="RigidBody"/>.</param>
		/// <param name="position">The location, in world-coordinates, to apply the force.</param>
		public void ApplyWorldForce(ref Vector3 force, ref Vector3 position)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Apply the specified force at the specified location to the total.
#if INLINE
			this.Force.X = this.Force.X + force.X;
			this.Force.Y = this.Force.Y + force.Y;
			this.Force.Z = this.Force.Z + force.Z;

			position.X = position.X - this.Position.X;
			position.Y = position.Y - this.Position.Y;
			position.Z = position.Z - this.Position.Z;

			this.Torque.X = this.Torque.X + (position.Y * force.Z - position.Z * force.Y);
			this.Torque.Y = this.Torque.Y + (position.Z * force.X - position.X * force.Z);
			this.Torque.Z = this.Torque.Z + (position.X * force.Y - position.Y * force.X);
#elif REFERENCE
			Vector3.Add(ref this.Force, ref force, out this.Force);

			Vector3 temp;
			Vector3.Subtract(ref position, ref this.Position, out temp);
			Vector3.Cross(ref temp, ref force, out temp);
			Vector3.Add(ref this.Torque, ref temp, out this.Torque);
#else
			this.Force += force;
			this.Torque += Vector3.Cross(position - this.Position, force);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies additional torque to the <see cref="RigidBody" /> in world coordinates.
		/// </summary>
		/// <param name="torque">The torque to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyWorldTorque(Vector3 torque)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified torque to the total.
#if INLINE
			this.Torque.X = this.Torque.X + torque.X;
			this.Torque.Y = this.Torque.Y + torque.Y;
			this.Torque.Z = this.Torque.Z + torque.Z;
#elif REFERENCE
			Vector3.Add(ref this.Torque, ref torque, out this.Torque);
#else
			this.Torque += torque;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies additional torque to the <see cref="RigidBody" /> in world coordinates.
		/// </summary>
		/// <param name="torque">The torque to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyWorldTorque(ref Vector3 torque)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Add the specified torque to the total.
#if INLINE
			this.Torque.X = this.Torque.X + torque.X;
			this.Torque.Y = this.Torque.Y + torque.Y;
			this.Torque.Z = this.Torque.Z + torque.Z;
#elif REFERENCE
			Vector3.Add(ref this.Torque, ref torque, out this.Torque);
#else
			this.Torque += torque;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies an impulse to the <see cref="RigidBody" /> at the specified location in world-coordinates.
		/// </summary>
		/// <param name="impulse">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		/// <param name="position">The location, in world-coordinates, to apply the impulse.</param>
		public void ApplyForceImpulse(Vector3 impulse, Vector3 position)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocities by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X + impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y + impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z + impulse.Z * this.inverseMass;

			position.X = position.X - this.Position.X;
			position.Y = position.Y - this.Position.Y;
			position.Z = position.Z - this.Position.Z;

			float x = position.Y * impulse.Z - position.Z * impulse.Y;
			float y = position.Z * impulse.X - position.X * impulse.Z;
			float z = position.X * impulse.Y - position.Y * impulse.X;

			this.Rotation.X = this.Rotation.X + (x * this.inverseInertiaTensorWorld.M11 + y * this.inverseInertiaTensorWorld.M21 + z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (x * this.inverseInertiaTensorWorld.M12 + y * this.inverseInertiaTensorWorld.M22 + z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (x * this.inverseInertiaTensorWorld.M13 + y * this.inverseInertiaTensorWorld.M23 + z * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			// Adjust the rotation by the specified impulse.
			Vector3.Subtract(ref position, ref this.Position, out position);
			Vector3.Cross(ref position, ref impulse, out position);
			Vector3.TransformNormal(ref position, ref this.inverseInertiaTensorWorld, out position);
			Vector3.Add(ref this.Rotation, ref position, out this.Rotation);

			// Adjust the velocity by the specified impulse.
			Vector3.Multiply(ref impulse, this.inverseMass, out impulse);
			Vector3.Add(ref this.Velocity, ref impulse, out this.Velocity);
#else
			this.Velocity += impulse * this.inverseMass;
			this.Rotation += Vector3.TransformNormal(Vector3.Cross(position - this.Position, impulse), this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies an impulse to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="impulse">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyForceImpulse(Vector3 impulse)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocity by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X + impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y + impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z + impulse.Z * this.inverseMass;
#elif REFERENCE
			Vector3.Multiply(ref impulse, this.inverseMass, out impulse);
			Vector3.Add(ref this.Velocity, ref impulse, out this.Velocity);
#else
			this.Velocity += impulse * this.inverseMass;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies an impulse to the <see cref="RigidBody" /> at the <see cref="RigidBody.Position"/>.
		/// </summary>
		/// <param name="impulse">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyForceImpulse(ref Vector3 impulse)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocity by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X + impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y + impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z + impulse.Z * this.inverseMass;
#elif REFERENCE
			Vector3 temp;
			Vector3.Multiply(ref impulse, this.inverseMass, out temp);
			Vector3.Add(ref this.Velocity, ref temp, out this.Velocity);
#else
			this.Velocity += impulse * this.inverseMass;
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies an impulse to the <see cref="RigidBody" /> at the specified location in world-coordinates.
		/// </summary>
		/// <param name="impulse">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		/// <param name="position">The location, in world-coordinates, to apply the impulse.</param>
		public void ApplyForceImpulse(ref Vector3 impulse, ref Vector3 position)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocities by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X + impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y + impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z + impulse.Z * this.inverseMass;

			float x = position.X - this.Position.X;
			float y = position.Y - this.Position.Y;
			float z = position.Z - this.Position.Z;

			float cx = y * impulse.Z - z * impulse.Y;
			float cy = z * impulse.X - x * impulse.Z;
			float cz = x * impulse.Y - y * impulse.X;

			this.Rotation.X = this.Rotation.X + (cx * this.inverseInertiaTensorWorld.M11 + cy * this.inverseInertiaTensorWorld.M21 + cz * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (cx * this.inverseInertiaTensorWorld.M12 + cy * this.inverseInertiaTensorWorld.M22 + cz * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (cx * this.inverseInertiaTensorWorld.M13 + cy * this.inverseInertiaTensorWorld.M23 + cz * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			Vector3 temp;

			Vector3.Multiply(ref impulse, this.inverseMass, out temp);
			Vector3.Add(ref this.Velocity, ref temp, out this.Velocity);

			Vector3.Subtract(ref position, ref this.Position, out temp);
			Vector3.Cross(ref temp, ref impulse, out temp);
			Vector3.TransformNormal(ref temp, ref this.inverseInertiaTensorWorld, out temp);
			Vector3.Add(ref this.Rotation, ref temp, out this.Rotation);
#else
			this.Velocity += impulse * this.inverseMass;
			this.Rotation += Vector3.TransformNormal(Vector3.Cross(position - this.Position, impulse), this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies impulsive torque to the <see cref="RigidBody"/> in world-coordinates.
		/// </summary>
		/// <param name="torque">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyTorqueImpulse(Vector3 torque)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the rotation by the specified impulse.
#if INLINE
			this.Rotation.X = this.Rotation.X + (torque.X * this.inverseInertiaTensorWorld.M11 + torque.Y * this.inverseInertiaTensorWorld.M21 + torque.Z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (torque.X * this.inverseInertiaTensorWorld.M12 + torque.Y * this.inverseInertiaTensorWorld.M22 + torque.Z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (torque.X * this.inverseInertiaTensorWorld.M13 + torque.Y * this.inverseInertiaTensorWorld.M23 + torque.Z * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			Vector3.TransformNormal(ref torque, ref this.inverseInertiaTensorWorld, out torque);
			Vector3.Add(ref this.Rotation, ref torque, out this.Rotation);
#else
			this.Rotation += Vector3.TransformNormal(torque, this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies impulsive torque to the <see cref="RigidBody"/> in world-coordinates.
		/// </summary>
		/// <param name="torque">The impulse, in world-coordinates, to apply to the <see cref="RigidBody"/>.</param>
		public void ApplyTorqueImpulse(ref Vector3 torque)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the rotation by the specified impulse.
#if INLINE
			this.Rotation.X = this.Rotation.X + (torque.X * this.inverseInertiaTensorWorld.M11 + torque.Y * this.inverseInertiaTensorWorld.M21 + torque.Z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (torque.X * this.inverseInertiaTensorWorld.M12 + torque.Y * this.inverseInertiaTensorWorld.M22 + torque.Z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (torque.X * this.inverseInertiaTensorWorld.M13 + torque.Y * this.inverseInertiaTensorWorld.M23 + torque.Z * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			Vector3 temp;
			Vector3.TransformNormal(ref torque, ref this.inverseInertiaTensorWorld, out temp);
			Vector3.Add(ref this.Rotation, ref temp, out this.Rotation);
#else
			this.Rotation += Vector3.TransformNormal(torque, this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Applies an impulse to the <see cref="RigidBody" /> at the specified offset from the position
		/// of the rigid body in world-coordinates.
		/// </summary>
		/// <param name="impulse">The impulse to apply to the <see cref="RigidBody"/>.</param>
		/// <param name="offset">The vector, in world-coordinates, from the the <see cref="RigidBody.Position"/>.</param>
		public void ApplyOffsetForceImpulse(ref Vector3 impulse, ref Vector3 offset)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocities by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X + impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y + impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z + impulse.Z * this.inverseMass;

			float x = offset.Y * impulse.Z - offset.Z * impulse.Y;
			float y = offset.Z * impulse.X - offset.X * impulse.Z;
			float z = offset.X * impulse.Y - offset.Y * impulse.X;

			this.Rotation.X = this.Rotation.X + (x * this.inverseInertiaTensorWorld.M11 + y * this.inverseInertiaTensorWorld.M21 + z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (x * this.inverseInertiaTensorWorld.M12 + y * this.inverseInertiaTensorWorld.M22 + z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (x * this.inverseInertiaTensorWorld.M13 + y * this.inverseInertiaTensorWorld.M23 + z * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			Vector3 temp;
			Vector3.Multiply(ref impulse, this.inverseMass, out temp);
			Vector3.Add(ref this.Velocity, ref temp, out this.Velocity);

			Vector3.Cross(ref offset, ref impulse, out temp);
			Vector3.TransformNormal(ref temp, ref this.inverseInertiaTensorWorld, out temp);
			Vector3.Add(ref this.Rotation, ref temp, out this.Rotation);
#else
			this.Velocity += impulse * this.inverseMass;
			this.Rotation += Vector3.TransformNormal(Vector3.Cross(offset, impulse), this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		// TODO : Remove or make internal, if able.  Seems too specialized.  I thought I was being clever until this.
		public void ApplyReverseOffsetForceImpulse(ref Vector3 impulse, ref Vector3 offset)
		{
			// Make sure the rigid body is not static.
			if ((this.Flags & RigidBodyFlags.Static) == RigidBodyFlags.Static)
			{
				return;
			}

			// Adjust the velocities by the specified impulse.
#if INLINE
			this.Velocity.X = this.Velocity.X - impulse.X * this.inverseMass;
			this.Velocity.Y = this.Velocity.Y - impulse.Y * this.inverseMass;
			this.Velocity.Z = this.Velocity.Z - impulse.Z * this.inverseMass;

			float x = impulse.Y * offset.Z - impulse.Z * offset.Y;
			float y = impulse.Z * offset.X - impulse.X * offset.Z;
			float z = impulse.X * offset.Y - impulse.Y * offset.X;

			this.Rotation.X = this.Rotation.X + (x * this.inverseInertiaTensorWorld.M11 + y * this.inverseInertiaTensorWorld.M21 + z * this.inverseInertiaTensorWorld.M31);
			this.Rotation.Y = this.Rotation.Y + (x * this.inverseInertiaTensorWorld.M12 + y * this.inverseInertiaTensorWorld.M22 + z * this.inverseInertiaTensorWorld.M32);
			this.Rotation.Z = this.Rotation.Z + (x * this.inverseInertiaTensorWorld.M13 + y * this.inverseInertiaTensorWorld.M23 + z * this.inverseInertiaTensorWorld.M33);
#elif REFERENCE
			Vector3 temp;
			Vector3.Multiply(ref impulse, this.inverseMass, out temp);
			Vector3.Subtract(ref this.Velocity, ref temp, out this.Velocity);

			Vector3.Cross(ref impulse, ref offset, out temp);
			Vector3.TransformNormal(ref temp, ref this.inverseInertiaTensorWorld, out temp);
			Vector3.Add(ref this.Rotation, ref temp, out this.Rotation);
#else
			this.Velocity -= impulse * this.inverseMass;
			this.Rotation += Vector3.TransformNormal(Vector3.Cross(impulse, offset), this.inverseInertiaTensorWorld);
#endif
			this.Assert();

			// Enable the rigid body.
			this.Activate();
		}

		/// <summary>
		/// Determines the mass and intertia tensor based on the specified density and associated <see cref="CollisionGroup"/>.
		/// </summary>
		/// <param name="density">The density of the <see cref="RigidBody"/>.</param>
		public void CalculateMassProperties(float density)
		{
			// Get the mass properties from the associated collision group.
			float mass;
			Matrix inertiaTensor;
			this.collisionGroup.CalculateMassProperties(density, out mass, out inertiaTensor);

			// Update the mass and interia tensor.
			this.Mass = mass;
			this.InertiaTensor = inertiaTensor;

			// Calculate the inertia tensor in world coordinates.
#if QUATERNION_ORIENTATION
			this.inverseInertiaTensorWorld = MatrixExtension.ConvertBasis33(this.inverseInertiaTensor, Matrix.CreateFromQuaternion(this.Orientation));
#else
			this.inverseInertiaTensorWorld = MatrixExtension.ConvertBasis33(this.inverseInertiaTensor, this.Orientation);
#endif
		}
	}

	internal static class RigidBodyExtension
	{
		[System.Diagnostics.Conditional("ASSERT")]
		internal static void Assert(this RigidBody source)
		{
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Position.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Position.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Position.Z));
#if QUATERNION_ORIENTATION
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.Z));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.W));
#else
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M11));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M12));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M13));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M14));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M21));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M22));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M23));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M24));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M31));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M32));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M33));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M34));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M41));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M42));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M43));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Orientation.M44));
#endif
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Velocity.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Velocity.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Velocity.Z));

			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Rotation.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Rotation.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Rotation.Z));

			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Force.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Force.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Force.Z));

			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Torque.X));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Torque.Y));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.Torque.Z));

			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M11));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M12));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M13));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M14));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M21));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M22));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M23));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M24));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M31));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M32));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M33));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M34));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M41));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M42));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M43));
			System.Diagnostics.Debug.Assert(!float.IsNaN(source.inverseInertiaTensorWorld.M44));
		}
	}
}
